import numpy as np

def generate_data(ym_per_pix, xm_per_pix):
    '''
    将上面代码生成的曲率的虚假数据用来生成车道曲率
    '''

    np.random.seed(0)
    
    ploty = np.linspace(0, 719, num=720)
    quadratic_coeff = 3e-4

    leftx = np.array([200 + (y**2)*quadratic_coeff + np.random.randint(-50, high=51) 
                                    for y in ploty])
    rightx = np.array([900 + (y**2)*quadratic_coeff + np.random.randint(-50, high=51) 
                                    for y in ploty])

    leftx = leftx[::-1] 
    rightx = rightx[::-1]  

    # 将二姐多项式拟合到每条车道的像素位置
    # 坐标拟合到世界空间的x, y
    left_fit_cr = np.polyfit(ploty * ym_per_pix, leftx * xm_per_pix, 2)
    right_fit_cr = np.polyfit(ploty * ym_per_pix, rightx * xm_per_pix, 2)
    
    return ploty, left_fit_cr, right_fit_cr
    
def measure_curvature_real():
    '''
    转化为米进制单位
    '''
    # 定了一从像素空间到米的转换方法
    # y 维度上的每像素米数
    ym_per_pix = 30/720 
    
    # y 维度上的每像素米数
    xm_per_pix = 3.7/700 
    
    # 获取曲率
    ploty, left_fit_cr, right_fit_cr = generate_data(ym_per_pix, xm_per_pix)
    
    # 定义曲率半径的最大值 对应于图象的底部
    y_eval = np.max(ploty)
    
    # 计算曲率半径
    left_curverad = ((1 + (2*left_fit_cr[0]*y_eval*ym_per_pix + left_fit_cr[1])**2)**1.5) / np.absolute(2*left_fit_cr[0])
    right_curverad = ((1 + (2*right_fit_cr[0]*y_eval*ym_per_pix + right_fit_cr[1])**2)**1.5) / np.absolute(2*right_fit_cr[0])
    
    return left_curverad, right_curverad



left_curverad, right_curverad = measure_curvature_real()

print("%.2f" % left_curverad, "m")
print("%.2f" % right_curverad, "m")
